/*
**Copyright (C) 2022, HITCRT_VISION, all rights reserved.
*/
#pragma once
#include "KF.h"
/**
 * @description:
 * EKF极坐标模型，观测输入measure各分量依次是[弧度（单位不是角度），距离，角速度，径向速度]，
 * 输入至少应该包含前两个维度，状态量包括[px,py,vx,vy,ax,ay]
 * 至少包含前四个状态量，另外输出量在getResult()方法中对后验状态估计做了转换，
 * 输出与measure维度一致，前两个分量即为滤波后的角度和距离
 * @author: WWW
 * @mail: 1922039181@qq.com
 */
namespace hitcrt {
class EKF : public KF {
  public:
    EKF(const int measureDim, const int stateDim) : KF(measureDim, stateDim){};
    ~EKF() = default;
    virtual void init(const std::vector<boost::any> &param) override;
    virtual void setParam(const std::vector<boost::any> &param) override;
    virtual void predict() override;
    virtual const cv::Mat &getResult() override;

  private:
    void setTransition();
    void setTransform(const cv::Mat &state);
    void setMeasureMatrix();
    const double MINI = 1e-7;
};
} // namespace hitcrt
